#pragma once

#include "integration_base.h"
#include "vins_so/utility/utility.h"
#include <ceres/ceres.h>
#include <eigen3/Eigen/Dense>
#include <iostream>
#include <ros/assert.h>

class IMUFactor : public ceres::SizedCostFunction< 15, 7, 3, 6, 7, 3, 6 >
{
    public:
    IMUFactor( ) = delete;
    IMUFactor( ImuPreintPtr _pre_integration )
    : m_pImuPreint( _pre_integration )
    {
    }
    virtual bool Evaluate( double const* const* parameters, double* residuals, double** jacobians ) const
    {

        Eigen::Vector3d Pi( parameters[0][0], parameters[0][1], parameters[0][2] );
        Eigen::Quaterniond Qi( parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5] );

        Eigen::Vector3d Vi( parameters[1][0], parameters[1][1], parameters[1][2] );

        Eigen::Vector3d Bai( parameters[2][0], parameters[2][1], parameters[2][2] );
        Eigen::Vector3d Bgi( parameters[2][3], parameters[2][4], parameters[2][5] );

        Eigen::Vector3d Pj( parameters[3][0], parameters[3][1], parameters[3][2] );
        Eigen::Quaterniond Qj( parameters[3][6], parameters[3][3], parameters[3][4], parameters[3][5] );

        Eigen::Vector3d Vj( parameters[4][0], parameters[4][1], parameters[4][2] );

        Eigen::Vector3d Baj( parameters[5][0], parameters[5][1], parameters[5][2] );
        Eigen::Vector3d Bgj( parameters[5][3], parameters[5][4], parameters[5][5] );

// Eigen::Matrix<double, 15, 15> Fd;
// Eigen::Matrix<double, 15, 12> Gd;

// Eigen::Vector3d pPj = Pi + Vi * sum_t - 0.5 * g * sum_t * sum_t + corrected_delta_p;
// Eigen::Quaterniond pQj = Qi * delta_q;
// Eigen::Vector3d pVj = Vi - g * sum_t + corrected_delta_v;
// Eigen::Vector3d pBaj = Bai;
// Eigen::Vector3d pBgj = Bgi;

// Vi + Qi * delta_v - g * sum_dt = Vj;
// Qi * delta_q = Qj;

// delta_p = Qi.inverse() * (0.5 * g * sum_dt * sum_dt + Pj - Pi);
// delta_v = Qi.inverse() * (g * sum_dt + Vj - Vi);
// delta_q = Qi.inverse() * Qj;

#if 0
        if ((Bai - pre_integration->linearized_ba).norm() > 0.10 ||
            (Bgi - pre_integration->linearized_bg).norm() > 0.01)
        {
            pre_integration->repropagate(Bai, Bgi);
        }
#endif

        Eigen::Map< Eigen::Matrix< double, 15, 1 > > residual( residuals );
        residual = m_pImuPreint->evaluate( Pi, Qi, Vi, Bai, Bgi, Pj, Qj, Vj, Baj, Bgj );

        Eigen::Matrix< double, 15, 15 > sqrt_info
        = Eigen::LLT< Eigen::Matrix< double, 15, 15 > >( m_pImuPreint->covariance.inverse( ) )
          .matrixL( )
          .transpose( );
        // sqrt_info.setIdentity();
        residual = sqrt_info * residual;

        if ( jacobians )
        {
            double sum_dt          = m_pImuPreint->sum_dt;
            Eigen::Matrix3d dp_dba = m_pImuPreint->jacobian.template block< 3, 3 >( O_P, O_BA );
            Eigen::Matrix3d dp_dbg = m_pImuPreint->jacobian.template block< 3, 3 >( O_P, O_BG );

            Eigen::Matrix3d dq_dbg = m_pImuPreint->jacobian.template block< 3, 3 >( O_R, O_BG );

            Eigen::Matrix3d dv_dba = m_pImuPreint->jacobian.template block< 3, 3 >( O_V, O_BA );
            Eigen::Matrix3d dv_dbg = m_pImuPreint->jacobian.template block< 3, 3 >( O_V, O_BG );

            if ( m_pImuPreint->jacobian.maxCoeff( ) > 1e8 || m_pImuPreint->jacobian.minCoeff( ) < -1e8 )
            {
                ROS_WARN( "numerical unstable in preintegration" );
                // std::cout << pre_integration->jacobian << std::endl;
                ///                ROS_BREAK();
            }

            if ( jacobians[0] )
            {
                Eigen::Map< Eigen::Matrix< double, 15, 7, Eigen::RowMajor > > jacobian_pose_i(
                jacobians[0] );
                jacobian_pose_i.setZero( );

                jacobian_pose_i.block< 3, 3 >( O_P, O_P ) = -Qi.inverse( ).toRotationMatrix( );
                jacobian_pose_i.block< 3, 3 >( O_P, O_R ) = Utility::skewSymm(
                Qi.inverse( ) * ( 0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt ) );

#if 0
            jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix();
#else
                Eigen::Quaterniond corrected_delta_q
                = m_pImuPreint->delta_q
                  * Utility::deltaQ( dq_dbg * ( Bgi - m_pImuPreint->linearized_bg ) );
                jacobian_pose_i.block< 3, 3 >( O_R, O_R )
                = -( Utility::Qleft( Qj.inverse( ) * Qi ) * Utility::Qright( corrected_delta_q ) )
                   .bottomRightCorner< 3, 3 >( );
#endif

                jacobian_pose_i.block< 3, 3 >( O_V, O_R )
                = Utility::skewSymm( Qi.inverse( ) * ( G * sum_dt + Vj - Vi ) );

                jacobian_pose_i = sqrt_info * jacobian_pose_i;

                if ( jacobian_pose_i.maxCoeff( ) > 1e8 || jacobian_pose_i.minCoeff( ) < -1e8 )
                {
                    ROS_WARN( "numerical unstable in preintegration" );
                    // std::cout << sqrt_info << std::endl;
                    ROS_BREAK( );
                }
            }
            if ( jacobians[1] )
            {
                Eigen::Map< Eigen::Matrix< double, 15, 3, Eigen::RowMajor > > jacobian_speed_i(
                jacobians[1] );

                jacobian_speed_i.setZero( );
                jacobian_speed_i.block< 3, 3 >( O_P, O_V - O_V )
                = -Qi.inverse( ).toRotationMatrix( ) * sum_dt;

                jacobian_speed_i.block< 3, 3 >( O_V, O_V - O_V ) = -Qi.inverse( ).toRotationMatrix( );

                jacobian_speed_i = sqrt_info * jacobian_speed_i;

                ROS_ASSERT( fabs( jacobian_speed_i.maxCoeff( ) ) < 1e8 );
                ROS_ASSERT( fabs( jacobian_speed_i.minCoeff( ) ) < 1e8 );
            }
            if ( jacobians[2] )
            {
                Eigen::Map< Eigen::Matrix< double, 15, 6, Eigen::RowMajor > > jacobian_bias_i(
                jacobians[2] );

                jacobian_bias_i.setZero( );
                jacobian_bias_i.block< 3, 3 >( O_P, O_BA - O_BA ) = -dp_dba;
                jacobian_bias_i.block< 3, 3 >( O_P, O_BG - O_BA ) = -dp_dbg;

#if 0
            jacobian_bias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg;
#else
                Eigen::Quaterniond corrected_delta_q
                = m_pImuPreint->delta_q
                  * Utility::deltaQ( dq_dbg * ( Bgi - m_pImuPreint->linearized_bg ) );
                jacobian_bias_i.block< 3, 3 >( O_R, O_BG - O_BA )
                = -Utility::Qleft( Qj.inverse( ) * Qi * corrected_delta_q ).bottomRightCorner< 3, 3 >( )
                  * dq_dbg;
#endif

                jacobian_bias_i.block< 3, 3 >( O_V, O_BA - O_BA ) = -dv_dba;
                jacobian_bias_i.block< 3, 3 >( O_V, O_BG - O_BA ) = -dv_dbg;

                jacobian_bias_i.block< 3, 3 >( O_BA, O_BA - O_BA ) = -Eigen::Matrix3d::Identity( );

                jacobian_bias_i.block< 3, 3 >( O_BG, O_BG - O_BA ) = -Eigen::Matrix3d::Identity( );

                jacobian_bias_i = sqrt_info * jacobian_bias_i;

                ROS_ASSERT( fabs( jacobian_bias_i.maxCoeff( ) ) < 1e8 );
                ROS_ASSERT( fabs( jacobian_bias_i.minCoeff( ) ) < 1e8 );
            }
            if ( jacobians[3] )
            {
                Eigen::Map< Eigen::Matrix< double, 15, 7, Eigen::RowMajor > > jacobian_pose_j(
                jacobians[3] );
                jacobian_pose_j.setZero( );

                jacobian_pose_j.block< 3, 3 >( O_P, O_P ) = Qi.inverse( ).toRotationMatrix( );

#if 0
            jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity();
#else
                Eigen::Quaterniond corrected_delta_q
                = m_pImuPreint->delta_q
                  * Utility::deltaQ( dq_dbg * ( Bgi - m_pImuPreint->linearized_bg ) );
                jacobian_pose_j.block< 3, 3 >( O_R, O_R )
                = Utility::Qleft( corrected_delta_q.inverse( ) * Qi.inverse( ) * Qj ).bottomRightCorner< 3, 3 >( );
#endif

                jacobian_pose_j = sqrt_info * jacobian_pose_j;

                ROS_ASSERT( fabs( jacobian_pose_j.maxCoeff( ) ) < 1e8 );
                ROS_ASSERT( fabs( jacobian_pose_j.minCoeff( ) ) < 1e8 );
            }
            if ( jacobians[4] )
            {
                Eigen::Map< Eigen::Matrix< double, 15, 3, Eigen::RowMajor > > jacobian_speed_j(
                jacobians[4] );

                jacobian_speed_j.setZero( );

                jacobian_speed_j.block< 3, 3 >( O_V, O_V - O_V ) = Qi.inverse( ).toRotationMatrix( );

                jacobian_speed_j = sqrt_info * jacobian_speed_j;

                ROS_ASSERT( fabs( jacobian_speed_j.maxCoeff( ) ) < 1e8 );
                ROS_ASSERT( fabs( jacobian_speed_j.minCoeff( ) ) < 1e8 );
            }
            if ( jacobians[5] )
            {
                Eigen::Map< Eigen::Matrix< double, 15, 6, Eigen::RowMajor > > jacobian_bias_j(
                jacobians[5] );
                jacobian_bias_j.setZero( );

                jacobian_bias_j.block< 3, 3 >( O_BA, O_BA - O_BA ) = Eigen::Matrix3d::Identity( );

                jacobian_bias_j.block< 3, 3 >( O_BG, O_BG - O_BA ) = Eigen::Matrix3d::Identity( );

                jacobian_bias_j = sqrt_info * jacobian_bias_j;

                ROS_ASSERT( fabs( jacobian_bias_j.maxCoeff( ) ) < 1e8 );
                ROS_ASSERT( fabs( jacobian_bias_j.minCoeff( ) ) < 1e8 );
            }
        }

        return true;
    }

    // bool Evaluate_Direct(double const *const *parameters, Eigen::Matrix<double, 15, 1>
    // &residuals, Eigen::Matrix<double, 15, 30> &jacobians);

    // void checkCorrection();
    // void checkTransition();
    // void checkJacobian(double **parameters);
    ImuPreintPtr m_pImuPreint;
};
